/**
 * @brief 		Chassis.h库文件
 * @detail
 * @author 	    Haoqi Liu
 * @date        25-5-23
 * @version 	V1.0.0
 * @note 		
 * @warning	    
 * @par 		历史版本
                V1.0.0创建于25-5-23
 * */

#ifndef CHASSIS_H
#define CHASSIS_H

#include "motor_servo.h"

class Chassis {
public:
    Chassis(const float freq,
            const motor_servo& servo1,
            const motor_servo& servo2) :
        freq(freq),
        speed_x(0),
        speed_y(0),
        speed_z(0),
        servos{servo1, servo2} {}

    float freq;    // 控制频率,单位:Hz
    float speed_x; // 单位:m/s
    float speed_y; // 单位:m/s
    float speed_z; // 单位:rpm

    bool initialized{false};
    bool enabled{false};
    bool started{false};

    bool initialize();
    bool enable();
    bool disable();
    bool start();
    bool stop();

    void ctrl();

private:
    motor_servo servos[2];
    bool first_run{true};
    float target_angles[2]{};
};

#endif //CHASSIS_H
